#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

using namespace Eigen;
using namespace std;

// B: Body
// C: Camera
// W: World
// L: Lidar
// R: Roar head

int main(int argc, char **argv)
{
    Quaterniond q_WR(0.55, 0.3, 0.2, 0.2), q_RB(0.99, 0, 0, 0.01), q_BL(0.3, 0.5, 0, 20.1), q_BC(0.8, 0.2, 0.1, 0.1);
    Vector3d t_WR(0.1, 0.2, 0.3), t_RB(0.05, 0, 0.5), t_BL(0.4, 0, 0.5), t_BC(0.5, 0.1, 0.5);
    Vector3d p_camera(0.3, 0.2, 1.2);

    q_WR.normalize();
    q_RB.normalize();
    q_BL.normalize();
    q_BC.normalize();

    Isometry3d T_WR(q_WR), T_RB(q_RB), T_BL(q_BL), T_BC(q_BC);
    T_WR.pretranslate(t_WR);
    T_RB.pretranslate(t_RB);
    T_BL.pretranslate(t_BL);
    T_BC.pretranslate(t_BC);


    Vector3d p_lidar = T_BL *  T_BC.inverse() * p_camera;

    Vector3d p_world = T_WR.inverse() * T_RB.inverse() * (T_BL.inverse() * p_lidar);

    cout << "p_linear: \n"<< p_lidar.transpose() << endl;
    cout << "p_world: \n"
         << p_world.transpose() << endl;

    return 0;
}
